#include "World.h"
#include "glutKeyboardManager.h"

World::World(void):
_spring1(Vector3<float>(140,100,140), Vector3<float>(0,0,0), 10),
_spring2(Vector3<float>(-140,100,-140), Vector3<float>(0,0,0), 10),
_pause(false)
{
}


World::~World(void)
{
	for(int i = 0; i < _objects.size(); i++)
	{
		delete _objects[i];
	}

	delete _player;

	delete _test;

	delete _target1;
	delete _target2;
}


void World::Update()
{
	if(!_pause)
	{
		_player->Update();
		_player->TreeManagment(_test);
	
		std::vector<Colider*> pObjects = _test->InBox(_player->Model());

		for(int i = 0; i < pObjects.size(); i++)
		{
			Vector3<float> myC, objC, cn; 
			float ol = FLT_MAX;
			if(_player->Model()->SATTest(pObjects[i], myC, objC, cn, ol))
			{
				_player->Translate(cn * -ol);
			}
		}

		Vector3<float> myColide, objColide, colisionNormal; 
		float overLap = FLT_MAX;
		for(int i = 0; i < 10; i++)
		{
			if(_player->_bullets[i].Active())
			{
				if(_player->_bullets[i].Model()->SATTest(_target1, myColide, objColide, colisionNormal, overLap))
				{
					_target1->AddForce(colisionNormal * 20000, objColide);
					_player->_bullets[i].Active() = false;
				}
				overLap = FLT_MAX;
		
				if(_player->_bullets[i].Model()->SATTest(_target2, myColide, objColide, colisionNormal, overLap))
				{
					_target2->AddForce(colisionNormal * 20000, objColide);
					_player->_bullets[i].Active() = false;
				}
			}
		}

	
		if(_player->Model()->SATTest(_target1, myColide, objColide, colisionNormal, overLap))
		{
			Colider::ForceColide(_player->Model(),myColide, _target1, objColide, colisionNormal, overLap);
		}
		if(_player->Model()->SATTest(_target2, myColide, objColide, colisionNormal, overLap))
		{
			Colider::ForceColide(_player->Model(),myColide, _target2, objColide, colisionNormal, overLap);
		}
		_spring1.End = _target1->PointOnShape(Vector3<float>(1,1,1)) + _target1->Position();
		_target1->AddForce(_spring1.getForce(), _target1->PointOnShape(Vector3<float>(1,1,1)));
		_target1->AddForce(Vector3<float>(0,-200,0),Vector3<float>::ZERO);

		_spring2.End = _target2->PointOnShape(Vector3<float>(1,1,1)) + _target2->Position();
		_target2->AddForce(_spring2.getForce(), _target2->PointOnShape(Vector3<float>(1,1,1)));
		_target2->AddForce(Vector3<float>(0,-200,0),Vector3<float>::ZERO);

		_target1->Update();
		_target2->Update();
	}

	if(glutKeyboardManager::FirstPress('p'))
	{
		_pause = !_pause;
	}
}

void World::Draw()
{
	for(int i = 0; i < _objects.size(); i++)
	{
		_objects[i]->Draw();
		//_objects[i].ManualDraw();
		//_objects[i].DrawNormals();
	}
	//_test->Draw();

	_player->Draw();


	_spring1.Draw();
	_spring2.Draw();

	bezCurve.Draw();
	bezSurf.Draw();
}

void World::init()
{
	// Load Objects
	ObjectLoader::LoadObjects(_objects);

	_player = new Player(Vector3<float>(-100,40,-100));

	_test = new OctTree(Vector3<float>(-210,-20,-210),420,420,420);

	for(int i = 0; i < _objects.size(); i++)
	{
		_test->addGameObject(_objects[i]);
	}

	_test->addGameObject(_player->Model());

	_target1 = new  Colider(GameConstants::MODELS["Cube"], Vector3<float>(50, 100, 50), 2, Quaternion::AxisAngle(Vector3<float>(0,-1,-1), 0.5), Vector3<float>(10,10,10));
	_target1->_color = MyColor(1,0.5,0,1);
	

	_target2 = new Colider(GameConstants::MODELS["Cube"], Vector3<float>(140, 10, 140), 2, Quaternion::AxisAngle(Vector3<float>(1,1,0), 0.5), Vector3<float>(10,10,10));
	_target2->_color = MyColor(1,0.5,0,1);

	_spring1.End = _target2->PointOnShape(Vector3<float>(1,1,1));

	_objects.push_back(_target1);
	_test->addGameObject(_target1);
	_objects.push_back(_target2);
	_test->addGameObject(_target2);


	std::vector<Vector3<float>> beziPoints;

	beziPoints.push_back(Vector3<float>(0, 5, 0));
	beziPoints.push_back(Vector3<float>(20, 50, 20));
	beziPoints.push_back(Vector3<float>(40, 20, 70));
	beziPoints.push_back(Vector3<float>(100, 30, 0));

	std::vector<Vector3<float>> beziPoints1;
	beziPoints1.push_back(Vector3<float>(0, 0, 0));
	beziPoints1.push_back(Vector3<float>(25, 64, 55));
	beziPoints1.push_back(Vector3<float>(50, 0, 100));
	beziPoints1.push_back(Vector3<float>(75, 57, 20));
	beziPoints1.push_back(Vector3<float>(100, 0, 70));

	std::vector<Vector3<float>> beziPoints2;
	beziPoints2.push_back(Vector3<float>(0, 0, 0));
	beziPoints2.push_back(Vector3<float>(0, 25, 25));
	beziPoints2.push_back(Vector3<float>(22, 50, 90));
	beziPoints2.push_back(Vector3<float>(0, 75, 20));
	beziPoints2.push_back(Vector3<float>(0, 100, 30));


	bezSurf = BezierSurface(beziPoints1, beziPoints2);

	bezCurve = BezierCurve(beziPoints);
	bezCurve.CreatePoints();
}